'''
####
Date : 2025-1-8
Notes: 通讯
此脚本主要功能:
1. 新建线程与ZBot客户端进行通讯-数据交互
2. 监控相关寄存器值,做出指定动作。如:产品中的KT产线,台面的按钮（急停等）操作

####
'''

# -*- coding: utf-8 -*-
import logging
import os
import socket
import select
import struct
import time
import errno
import threading
import modbus_tk.defines as cst
import modbus_tk.modbus_tcp as modbus_tcp

from threading import Thread
from PyQt5.QtCore import QTimer

from processModbusTCP__ import process_Master
from processModbusRTU_ import rtu_Master
from processCommunicate_ import VSR_Server

### 日志文件
robot_logger=logging.getLogger("ZMove")
robot_logger.setLevel(logging.DEBUG)
### 默认显示
os.environ['DISPLAY'] = ':0'

process_master = process_Master()
rtu_master = rtu_Master()

class Controler_Server():

    def __init__(self, host = "0.0.0.0", port = 502):
        super().__init__()
        host_name = socket.gethostname()
        self.host_add = socket.gethostbyname(host_name)
        self.host = host
        self.port = port

        self.server_tcp_init()
        self.modbusClients = list()
        self.vsrserver = VSR_Server()
        self.server_th = Thread(target= self.vsrserver.run)
        self.server_th.start()

        self.th_update = Thread(target= self.status_update)
        self.th_update.start()

        self.monitor_server_socket_thread = Thread(target= self.monitor_server_socket, daemon=False)
        self.monitor_server_socket_thread.start()

    def server_tcp_init(self,):
        try:
            self.server = modbus_tcp.TcpServer(port=self.port, address=self.host, timeout_in_sec=5)
            self.server.start()
            self.register_init()
        
        except Exception as e:
            robot_logger.error(e,exc_info=True)

    def monitor_server_socket(self):
        ## 用于追踪已连接的客户端地址
        connected_clients = set()
        while True:
            ## 过滤掉无效的套接字
            valid_sockets = [s for s in self.server._sockets if s.fileno() >= 0]  
            try:
                inputready, _, _ = select.select(valid_sockets, [], [], 1.0)
                for sock in inputready:
                    if sock == self.server._sock: 
                        try:
                            client, address = sock.accept()
                            client.setblocking(0)
                            fileno = client.fileno()
                            addr_str = str(address)

                            ## 如果是新客户端，则打印连接信息并添加到已连接客户端集合中
                            if addr_str not in connected_clients:
                                connected_clients.add(addr_str)
                                self.modbusClients.append(address) 
                                self.server._sockets.append(client)

                        except OSError as e:
                            ## 忽略特定的非阻塞操作错误 [Errno 11] Resource temporarily unavailable 或 [Errno 35] Resource temporarily unavailable
                            if e.errno != errno.EAGAIN and e.errno != errno.EWOULDBLOCK and e.errno != errno.EINPROGRESS:
                                print(f"Unexpected exception occurred during accept(): {e}")
                                raise
            
            except select.error as e:
                ## 处理 select 错误...
                print(f"Select error occurred: {e}")
    
    def check_port_conflict(self,):
        try:
            ret = False
            sock = socket.socket(socket.AF_INET,socket.SOCK_STREAM)
            result = sock.connect_ex(self.host,self.port)
            if result == 0:
                ret = True
            else:
                ret = False
            sock.close()
            return ret
        
        except Exception as e:
            robot_logger.error(e,exc_info=True)
    
    def register_init(self,):
        self.slave =self.server.add_slave(1) 
        self.slave.add_block("robot6",cst.HOLDING_REGISTERS, 0, 600) 
        self.slave.add_block("var", cst.COILS, 0, 600)

    def int2float(self,a,b):
        f=0
        try:
            z0=hex(a)[2:].zfill(4) #取0x后边的部分 右对齐 左补零
            z1=hex(b)[2:].zfill(4) #取0x后边的部分 右对齐 左补零
            z=z0+z1 #高字节在前 低字节在后
            f=struct.unpack('!f', bytes.fromhex(z))[0] #返回浮点数
        
        except BaseException as e:
            print(e)
        return f

    def float_to_int16s(self,f):
        # 将浮点数打包成32位二进制数据
        b = struct.pack('f', f)
        # 将32位二进制数据拆分成两个16位二进制数据
        i1, i2 = struct.unpack('HH', b)
        return i2, i1

    def status_update(self,):
        while True:
            try:
                time.sleep(0.005)
                ### 更新机器人的状态
                status = []
                DI_status = []
                DO_status = []
                slider_status = []
                
                s = self.vsrserver.robot_control.status_global
                if s != None:
                    if s["state"] == "Idle":
                        status.append(1)
                    elif s["state"] == "Alarm":
                        status.append(2)
                    elif s["state"] == "Home":
                        status.append(3)
                    elif s["state"] == "Run":
                        status.append(4)
                    else:
                        status.append(0)
                    ang = s["angle"]
                    for i in range(len(ang)):
                        a = self.float_to_int16s(ang[i])
                        status.append(a[0])
                        status.append(a[1])

                    ang = s["coordinate"]

                    for i in range(len(ang)):
                        a = self.float_to_int16s(ang[i])
                        status.append(a[0])
                        status.append(a[1])
                        if i ==6:
                            slider_status.append(a[0])
                            slider_status.append(a[1])
                    
                    ### 增加机器人的类型
                    status.append(self.vsrserver.robot_control.robot_type_number)     
                    ### 将机器人的状态更新到0号寄存器
                    self.slave.set_values("robot6", 0, status)
                    
                    ###DO 9个的值写入28---36寄存器地址
                    DO_Status = rtu_master.rtu_server.execute(slave=1, function_code=cst.READ_HOLDING_REGISTERS, starting_address=11, quantity_of_x= 9)
                    if DO_Status is not None and len(DO_Status) == 9:
                        for i in range(len(DO_Status)):
                            DO_status.append(DO_Status[i])
                            self.slave.set_values("robot6", 28, DO_status)
                    else:
                        # 如果获取的数据有问题，使用默认初始化值udo
                        DO_status.append([0] * 9)
                        self.slave.set_values("robot6", 28, DO_status)
                    
                    time.sleep(0.5)

                    DI_Status = rtu_master.rtu_server.execute(slave=1, function_code=cst.READ_HOLDING_REGISTERS, starting_address=0, quantity_of_x= 11)

                    if DI_Status is not None and len(DI_Status) == 11:
                        for i in range(len(DI_Status)):
                            DI_status.append(DI_Status[i])
                        self.slave.set_values("robot6", 37, DI_status)
                    else:
                        # 如果获取的数据有问题，使用默认初始化值
                        DI_status.append([0] * 11)
                        self.slave.set_values("robot6", 37, DI_status)
                    time.sleep(1)
                    
            
                ### 更新机器人2的状态
                status_2 = []
                slider_status_2 = []
                s_2 = self.vsrserver.robot_control.status_global_
                if s_2 != None:
                    if s_2["state"] == "Idle":
                        status_2.append(1)
                    elif s_2["state"] == "Alarm":
                        status_2.append(2)
                    elif s_2["state"] == "Home":
                        status_2.append(3)
                    elif s_2["state"] == "Run":
                        status_2.append(4)
                    else:
                        status_2.append(0)
                    ang = s_2["angle"]
                    for i in range(len(ang)):
                        a = self.float_to_int16s(ang[i])
                        status_2.append(a[0])
                        status_2.append(a[1])

                    coor = s_2["coordinate"]
                    for i in range(len(coor)):
                        a = self.float_to_int16s(coor[i])
                        status_2.append(a[0])
                        status_2.append(a[1])
                        if i ==6:
                            slider_status_2.append(a[0])
                            slider_status_2.append(a[1])
                    
                    ### 增加机器人的类型
                    status_2.append(self.vsrserver.robot_control.robot_type_number_2)     
                    # ### 增加数字仿真服务状态
                    # status_2.append(self.vsrserver.connect_flag)
                    ### IO状态
                    ### 将2号机器人的状态更新到100号寄存器
                    self.slave.set_values("robot6", 60, status_2)

            except Exception as e:
                robot_logger.error(e,exc_info=True)

    def dorun(self,):
        while True:
            try:
                time.sleep(0.1)
                cmd_di = self.slave.get_values("robot6", 73,2)
                if cmd_di[0] == 0:
                    pass
                elif cmd_di[0] == 1:
                    self.vsrserver.robot_control.robot_stop(com_Num = 1)
                    self.vsrserver.robot_control.robot_stop(com_Num = 2)
                    self.slave.set_values("robot6", 73, 0)
                
                if cmd_di[1] == 0:
                    pass
                elif cmd_di[1] == 1:
                    self.vsrserver.robot_control.robot_home(com_Num=1)
                    self.vsrserver.robot_control.robot_home(com_Num=2)
                    self.slave.set_values("robot6", 74, 0)


                ### 3执行机器人的指令
                time.sleep(0.05)
                cmd = self.slave.get_values("robot6", 99, 1)

                ### 3 Stop 1 Home 2 Zero
                if cmd[0] == 0:
                    pass
                elif cmd[0] == 1:
                    self.vsrserver.robot_control.home_position(com_Num=1)
                    self.vsrserver.robot_control.home_position(com_Num=2)
                    self.slave.set_values("robot6", 99, 0)
                elif cmd[0] == 2:
                    self.vsrserver.robot_control.go_to_zero()
                elif cmd[0] == 3:
                    self.vsrserver.robot_control.robot_stop(com_Num=1)
                    self.vsrserver.robot_control.robot_stop(com_Num=2)

                ### 4 控制台执行机器人的指令
                mini_factroy_cmd = self.slave.get_values("robot6", 37, 2)
                # mini_factroy_cmd = rtu_master.rtu_server.execute(1, cst.READ_HOLDING_REGISTERS, starting_address= 0, quantity_of_x=2)
                if mini_factroy_cmd[0] == 0 :
                    pass
                elif mini_factroy_cmd[0] == 1:
                    self.vsrserver.robot_control.robot_stop(com_Num=1)
                    self.vsrserver.robot_control.robot_stop(com_Num=2)
                
                if mini_factroy_cmd[1] == 0:
                    pass
                elif mini_factroy_cmd[1] == 1:
                    self.vsrserver.robot_control.home_position(com_Num=1)
                    self.vsrserver.robot_control.home_position(com_Num=2)
            
                ### 5Lua脚本的指令
                time.sleep(0.05)
                cmd = self.slave.get_values("robot6", 98, 1)

                ### 1.runLua 2. stopLua 3. terminalLua
                if cmd[0] == 0:
                    pass
                elif cmd[0] == 1:
                    self.vsrserver.lua_robotLua.runningLua = True
                    self.slave.set_values("robot6", 98, 0)
                elif cmd[0] == 2:
                    self.vsrserver.robot_control.allRobot_stop()            
                    self.vsrserver.lua_robotLua.stop_process()
                    rtu_master.rtu_server.execute(1, cst.WRITE_MULTIPLE_REGISTERS, starting_address=10, output_value=[0,0,0,0,0,0,0,0])
                    self.slave.set_values("robot6", 98, 0)
                elif cmd[0] == 3:
                    self.vsrserver.robot_control.allRobot_stop()
                    self.vsrserver.lua_robotLua.stop_process()
                    rtu_master.rtu_server.execute(1, cst.WRITE_MULTIPLE_REGISTERS, starting_address=10, output_value=[0,0,0,0,0,0,0,0])
                    self.slave.set_values("robot6", 98, 0)
            
            except Exception as e:
                robot_logger.error(e,exc_info=True)



#### 此版本不再执行main，仅引用Controler_Server类,执行到mainwindow.py中
if __name__ == '__main__':

    process_master = process_Master()
    rtu_master = rtu_Master()
    server = Controler_Server()
    server.dorun()



